//
// Created by rebeater on 4/27/21.
//

#include <stdio.h>
#include "rtcommon.h"
#include "agilex.h"
#define MAX_VELOCITY 3.0
#define MAC_ANGULAR 2.5235



/**
 * CAN数据校验
 * @param id
 * @param rxdata
 * @param len
 * @return
 */
uint8_t AgilexCheckSum(uint16_t id, const uint8_t rxdata[8], uint8_t len) {
  uint8_t checksum = 0x00;
  checksum = (uint8_t)(id & 0x00ffu) + (uint8_t)(id >> 8u) + len;
  for (int i = 0; i < (len - 1); i++) {
	checksum += rxdata[i];
  }
  return checksum;
}

/**
 * can data check
 * @param id
 * @param rxdata
 * @param len
 * @return
 */
uint8_t agilex_check(uint16_t id, const uint8_t rxdata[8], uint8_t len) {
  uint8_t checksum = AgilexCheckSum(id, rxdata, len);
  return checksum == rxdata[len - 1];
}
int agilexGetRobotInfo(RobotInfo *robotInfo, const FDCAN_RxHeaderTypeDef *header, const uint8_t data[8]) {
  if (!agilex_check(header->Identifier, data, header->DataLength)) {
	return -1;/**/
  }
  robotInfo->state = data[0];
  robotInfo->mode = data[1];
  robotInfo->battery = (((uint16_t)data[2] << 8u) + data[3]) / 10.0f;
  robotInfo->error = data[4];
  return 0;
}


FDCAN_TxHeaderTypeDef TxHeader;
uint8_t cmd_cnt = 0;
void FDCAN_Config() {
  FDCAN_FilterTypeDef sFilterConfig;
  sFilterConfig.IdType = FDCAN_STANDARD_ID;
  sFilterConfig.FilterIndex = 0;
  sFilterConfig.FilterType = FDCAN_FILTER_MASK;
  sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
  /*mask 只需要接受0x131速度信息和0x151机器人信息，所以使用0b1111 1001 1111作为mask，0x79F*/
  sFilterConfig.FilterID1 = AGILEX_VELOCITY_IDENTIFIER;
  sFilterConfig.FilterID2 = 0x79F; /* For acceptance, MessageID and FilterID1 must match exactly */
  HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilterConfig);
  /* Configure global filter to reject all non-matching frames */
  HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN_REJECT, FDCAN_REJECT, FDCAN_REJECT_REMOTE, FDCAN_REJECT_REMOTE);

  /* Configure Rx FIFO 0 watermark to 2 */
  HAL_FDCAN_ConfigFifoWatermark(&hfdcan1, FDCAN_CFG_RX_FIFO0, 2);

  /* Activate Rx FIFO 0 watermark notification */
  HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0);

  TxHeader.Identifier = 0x130;
  TxHeader.IdType = FDCAN_STANDARD_ID;
  TxHeader.TxFrameType = FDCAN_DATA_FRAME;
  TxHeader.DataLength = FDCAN_DLC_BYTES_8;
  TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
  TxHeader.BitRateSwitch = FDCAN_BRS_OFF;
  TxHeader.FDFormat = FDCAN_CLASSIC_CAN;
  TxHeader.TxEventFifoControl = FDCAN_STORE_TX_EVENTS;
  TxHeader.MessageMarker = 0x52;
  cmd_cnt = 0;
}

int sendVelocity(float forward,float angular) {
  /* Configure Tx buffer message */
  uint8_t TxData[8] = {0x01, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x44};
  TxData[0] = 0x01;
  TxData[1] = 0x00;
  TxData[2] = (int)(100.0 * forward / MAX_VELOCITY);
  TxData[3] = (int)(100.0 * angular / MAC_ANGULAR);
  TxData[4] = 0x00;
  TxData[5] = 0x00;
  TxData[6] = cmd_cnt;
  cmd_cnt++;
  TxData[7] = AgilexCheckSum(TxHeader.Identifier, TxData, 8);
  if (HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &TxHeader, TxData) != HAL_OK) {
	printf("transmit failed\n");
  };
  return 0;
}